package edu.centenary.centenaryController;
import java.io.*;
import java.util.*;

/**
* A robot AI designed for the specific sensors on the P2AT robot.  This AI tells the robot to turn when the Range Scanner (which covers a 180 degree arc in front of it) returns a small distance value.  It then turns until it reaches "desiredYaw" which is where the greatest distance to an object was found.
*/ 
public class P2ATAI extends SleeperThread implements Runnable {
	 /**
	 * Sends messages to the server
	 */
    private PrintWriter out;
	 
	 private PrintWriter fileOut;
	 
	 private String filePath = "victimpartsV3_11Standard1.txt";
	 /**
	 * Communicates with the SensorData method
	 */
    private SensorData sd;
	 /**
	 * Becomes true when the robot's range sensors return a value below a certain point
	 */
    private boolean tooClose = false;
	 /**
	 * The yaw (Z axis rotation) the robot should have before moving forward
	 */
    private double desiredYaw = 0;
	 /**
	 * Communicates with the ImageWriter method
	 */
	 private ImageWriter iw = null;
	 /**
	 * An array list that stores the waypoints, and it's x,y,z coordinates 
	 */
	 private ArrayList<WayPoint> waypoints = new ArrayList<WayPoint>();
	 
	 
    //f1 - f8 are the distance values (from left to right) for the stopping distance before turning
	 /**
	 * The value representing a sonar sensor on the front side of the P2AT robot
	 */
    private double f1 = 0;
	 /**
	 * The value representing a sonar sensor at a 45 degree angle on the front of the  P2AT robot
	 */
    private double f2 = 0.05;
	 /**
	 * The value representing a sonar sensor on the front of the P2AT robot
	 */
    private double f3 = 0.4;
	 /**
	 * The value representing a sonar sensor on the front of the P2AT robot
	 */
    private double f4 = 0.5;
	 /**
	 * The value representing a sonar sensor on the front of the P2AT robot
	 */
    private double f5 = 0.5;
	 /**
	 * The value representing a sonar sensor on the front of the P2AT robot
	 */
    private double f6 = 0.4;
	 /**
	 * The value representing a sonar sensor at a 45 degree angle on the front of the  P2AT robot
	 */
    private double f7 = 0.05;
	 /**
	 * The value representing a sonar sensor on the front side of the P2AT robot
	 */
    private double f8 = 0;
	 /**
	 * The value representing a sonar sensor on the rear side of the P2AT robot
	 */
    private double r1 = 0;
	 /**
	 * The value representing a sonar sensor at a 45 degree angle on the rear of the  P2AT robot
	 */
    private double r2 = 0;
	 /**
	 * The value representing a sonar sensor on the rear of the P2AT robot
	 */
    private double r3 = 0;
	 /**
	 * The value representing a sonar sensor on the rear of the P2AT robot
	 */
    private double r4 = 0;
	 /**
	 * The value representing a sonar sensor on the rear of the P2AT robot
	 */
    private double r5 = 0;
	 /**
	 * The value representing a sonar sensor on the rear of the P2AT robot
	 */
    private double r6 = 0;
	 /**
	 * The value representing a sonar sensor at a 45 degree angle on the rear of the  P2AT robot
	 */
    private double r7 = 0;
	 /**
	 * The value representing a sonar sensor on the rear side of the P2AT robot
	 */
    private double r8 = 0;
	 /** An array that stores the body parts seen by the viticm sensor */
    private ArrayList<BodyPart> parts = new ArrayList<BodyPart>();
	 
	 /**
	 * Constructor method for P2ATAI
	 * @param iw The image writer method object
	 * @param out A server communication PrintWriter
	 * @param sd The Sensor Data method object
	 */
    public P2ATAI(ImageWriter iw,PrintWriter out, SensorData sd, ArrayList<WayPoint> waypoints) {
	 		this.iw = iw;
        this.out = out;
		  System.out.println("Ready to make file");
		  try{
		  	this.fileOut = new PrintWriter(new FileWriter(filePath));
			}catch(Exception e){
				System.out.println("File not found");
				System.exit(-1);
			}
			System.out.println("File Made");
        this.sd = sd;
		  this.waypoints = waypoints;
		 
    }
	 /**
	 * Takes the robot's percieved body part locations and translates it into their actual location on the map
	 */
	 public void detectBodyParts(){
	 	ArrayList<BodyPart> partsTemp = sd.getBodyPartList();
		for(int i = 0; i < partsTemp.size(); i++){
			BodyPart limb = partsTemp.get(i);
			double bpxp = limb.getLocationX() * Math.cos(- sd.getYaw()) + limb.getLocationY() * Math.sin(- sd.getYaw());
   	   double bpyp = limb.getLocationY() * Math.cos(- sd.getYaw()) - limb.getLocationX() * Math.sin(- sd.getYaw());
     		BodyPart bp = new BodyPart(limb.getBodyPart(), (sd.getLocationX() + bpxp), (sd.getLocationY() + bpyp), (sd.getLocationZ() + limb.getLocationZ()));
			if(!parts.contains(bp)){
				parts.add(bp);
			}
		}
		
		
	 }
	 public void writeBodyParts(){
	 	for (BodyPart bp: parts){
			fileOut.println(bp);
			System.out.println(bp);
		}
		fileOut.close();
	 }
	 	 /**
	 * The main method method thread for P2ATAI. 
	 * It determines when the robot should drive and tells it to turn when it gets close to an obstacle.  
	 * The robot turns up to 90 degrees left or right where it percieves the greatest amount of distance to the next obstacle.
	 */
    public void run() {


        sleep(1000);
        System.out.println("Robot Start!");
			iw.setBodyPartList(parts);
        out.println("DRIVE {Right 4} {Left 4}\r\n");
        while (!Thread.currentThread().isInterrupted()) {
		  		detectBodyParts();

            if (sd.getSonarKey("F4")) {
                //System.out.println(sd.getSonar("F4"));
            }
            if (sd.getTouch() && !tooClose) {
					iw.setRecord(false);
                out.println("DRIVE {Right -0.5} {Left 0.5}\r\n");
                System.out.println("Turning!");
                tooClose = true;
                desiredYaw = (sd.getYaw() + 1.5707963267948965)%6.28;

            } else if (((sd.getSonarKey("F1") && sd.getSonar("F1") <= f1) ||
                        (sd.getSonarKey("F2") && sd.getSonar("F2") <= f2) ||
                        (sd.getSonarKey("F3") && sd.getSonar("F3") <= f3) ||
                        (sd.getSonarKey("F4") && sd.getSonar("F4") <= f4) ||
                        (sd.getSonarKey("F5") && sd.getSonar("F5") <= f5) ||
                        (sd.getSonarKey("F6") && sd.getSonar("F6") <= f6) ||
                        (sd.getSonarKey("F7") && sd.getSonar("F7") <= f7) ||
                        (sd.getSonarKey("F8") && sd.getSonar("F8") <= f8) ||
                        (sd.getSonarKey("R1") && sd.getSonar("R1") <= r1) ||
                        (sd.getSonarKey("R2") && sd.getSonar("R2") <= r2) ||
                        (sd.getSonarKey("R3") && sd.getSonar("R3") <= r3) ||
                        (sd.getSonarKey("R4") && sd.getSonar("R4") <= r4) ||
                        (sd.getSonarKey("R5") && sd.getSonar("R5") <= r5) ||
                        (sd.getSonarKey("R6") && sd.getSonar("R6") <= r6) ||
                        (sd.getSonarKey("R7") && sd.getSonar("R7") <= r7) ||
                        (sd.getSonarKey("R8") && sd.getSonar("R8") <= r8))
                       && !tooClose) {
					iw.setRecord(false);
                //out.println("SEND " + " " + "4 " + "Blue");
					 out.println("GETSS crazy0 moose0 \r\n");
                tooClose = true;
                double[] adjustedRangeScanner = new double[181];
					 for(int i = 0; i < 181; i++){
					 	adjustedRangeScanner[i] = sd.getRangeScanner(i-7);
					 	for(int j = i - 7; j < i +7; j++){
							if(sd.getRangeScanner(j) < adjustedRangeScanner[i]){
								adjustedRangeScanner[i] = sd.getRangeScanner(j);
							}
						}
					 }
                System.out.println("Turning!");
                //Go to random location
					 int numangles = 70;
                int[] degreeOfGreatDistance = new int[numangles];
					 int[] minimumDistance = new int[numangles];
					 //Set minimum distance to something that isn't zero
					 for(int i = 0; i < numangles; i++){
					 	minimumDistance[i] = 500;
					 }
					 	//Create 5 random degrees of great distance.
					   for(int i = 0; i < numangles; i++){
							degreeOfGreatDistance[i] = (int)(Math.random() * 181);
							//Find the closest waypoint to each degree of great distance
							for(int k = 0; k < waypoints.size(); k++){
								WayPoint wp = waypoints.get(k);
								double gdX = (double)sd.getLocationX()+(adjustedRangeScanner[degreeOfGreatDistance[i]] * Math.cos((((degreeOfGreatDistance[i]-90)* (Math.PI/180))- sd.getYaw())) + 0);
								double gdY = (double)sd.getLocationY()+(0 - adjustedRangeScanner[degreeOfGreatDistance[i]] * Math.sin((((degreeOfGreatDistance[i]-90)* (Math.PI/180)) - sd.getYaw())));
								//System.out.println("gdX is " + gdX + " gdY is " + gdY);
								//System.out.println("wpX is " + wp.getX() + " wpY is " + wp.getY());
								//System.out.println("Distance is " + Math.sqrt(Math.pow(gdX - wp.getX(), 2) + Math.pow(gdY -wp.getY(), 2)));
								if(Math.sqrt(Math.pow(gdX - wp.getX(), 2) + Math.pow(gdY -wp.getY(), 2)) < minimumDistance[i]){
									minimumDistance[i] = (int)Math.sqrt(Math.pow(gdX - wp.getX(), 2) + Math.pow(gdY -wp.getY(), 2));
									//System.out.println("Minimum distance " + i + " is " + minimumDistance[i]);
								}
								  
								
							}
						}
						//Find greatest distance out of all minimums
						int degreeOfGreatestDistance = 0;
						for(int i = 0; i < numangles; i++){
							for(int k = 0; k < numangles; k++){
								if(minimumDistance[i] > minimumDistance[k]){
									degreeOfGreatestDistance = degreeOfGreatDistance[i];
								}
							}
						}
						//System.out.println("Degree of Greatest Distance is " + degreeOfGreatestDistance);
					
                
                if (degreeOfGreatestDistance < 90) {
                    out.println("DRIVE {Right -0.5} {Left 0.5}\r\n");
                } else {
                    out.println("DRIVE {Right 0.5} {Left -0.5}\r\n");
                }
                desiredYaw = ((sd.getYaw() + (( 180 - degreeOfGreatestDistance) + 270) * (Math.PI/180) ))%6.28;
                //System.out.println("Scanner's Location is: " + degreeOfGreatestDistance);
                //System.out.println("Farthest location is: " + greatestDistance);
                //System.out.println("Yaw is: " + desiredYaw);
                //End go to farthest location algorithm

            }

            if (tooClose) {
                //out.println("DRIVE {Right 0} {Left 0}");
                //System.out.println(sd.getYaw());
					 
                if (Math.abs(sd.getYaw()- desiredYaw) < 0.1) {

                    out.println("DRIVE {Right 4} {Left 4}\r\n");
                    tooClose = false;
							iw.setRecord(true);

                }
            }

            sleep(100);

        }

    }
}



